﻿using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using DL.Tools;
using Tools.Extensions;
using UnityEngine;

namespace DL.D_Start_Lite
{
    public class DStartLite<T>
    {
        private readonly Node<T> m_StartNode;
        private readonly Node<T> m_GoalNode;

        private readonly List<Node<T>> m_AllNodes;
        //关键修改，可能使用钥匙打开了门或者跳过
        private float km;

        class KeyNodeComparer : IComparer<(Key,Node<T>)>
        {
            public int Compare((Key, Node<T>) x, (Key, Node<T>) y) =>
                x.Item1 < y.Item1 ? -1 : x.Item1 > y.Item1 ? 1 : 0;
        }
        
        //开启集合
        private readonly SortedSet<(Key, Node<T>)> m_OpenSet = new(new KeyNodeComparer());
        //查找集合
        private readonly Dictionary<Node<T>, Key> m_LookUps;


        public DStartLite(Node<T> startNode, Node<T> goalNode, List<Node<T>> allNodes)
        {
            m_StartNode = startNode;
            m_GoalNode = goalNode;
            m_AllNodes = allNodes;
        }
        
        //算法部分  
        public void Initialize()
        {
            Reset();
            
            var key = CalculateKey(m_GoalNode);
            m_OpenSet.Add((key, m_GoalNode));
            m_LookUps.Add(m_GoalNode, key);

        }

        //计算Key值
        private Key CalculateKey(Node<T> node)
        {
            //第一个为估计最 小花费 + 启发值 + km
            //第二个为最小花费
            //只要和起点离得近，这个值会变小
            return new(MathF.Min(node.G, node.RHS) + node.Heuristic(node, m_StartNode) + km,
                MathF.Min(node.G, node.RHS));
        }

        private void UpdateVertex(Node<T> node)
        {
            var key = CalculateKey(node);
            //实际值和预估值还未更新 并且 未处理过的(还未查找)。添加
            if (!node.GEqualRHS && !m_LookUps.ContainsKey(node))
            {
                m_OpenSet.Add((key, node));
                m_LookUps[node] = key;
            }
            //如果已经优化过,但是该节点还在我们的查找列表中。移除
            else if (node.GEqualRHS && m_LookUps.ContainsKey(node))
            {
                m_OpenSet.Remove((m_LookUps[node], node));
                m_LookUps.Remove(node);
            }
            //没有优化过，但是在查找列表中。优化
            else if(m_LookUps.ContainsKey(node))
            {
                //更新Key值 
                m_OpenSet.Remove((m_LookUps[node], node));
                m_OpenSet.Add((key, node));
                m_LookUps[node] = key;
            }
        }

        //最大循环次数
        private const int k_maxCycles = 1000;
        
        public void RecalculateNode(Node<T> node)
        {
            km += m_StartNode.Heuristic(m_StartNode, node);

            var allConnectedNodes = Successors(node).Concat(Predecessors(node)).ToList();

            foreach (var s in allConnectedNodes)
            {
                if (s != m_StartNode)
                {
                    s.RHS = Mathf.Min(s.RHS, s.Cost(s, node) + node.G);
                }
                UpdateVertex(s);
            }
            UpdateVertex(node);
            ComputeShortestPath();
        }

        //计算最短路径
        public void ComputeShortestPath()
        {
            int maxSteps = k_maxCycles;
            //开启列表存在元素，且 其中最小花费都大于开始节点的预估值或者起始点的预估值大于实际值
            while (m_OpenSet.Count > 0 &&
                   (m_OpenSet.Min.Item1 < CalculateKey(m_StartNode) || m_StartNode.RHS > m_StartNode.G))
            {
                if (maxSteps-- <= 0)
                {
                    Debugger.EO_LogWarning("ComputeShortestPath error: max steps exceeded.");
                    break;
                }

                //最小花费点
                var smallest = m_OpenSet.Min;
                var node = smallest.Item2;
                m_OpenSet.Remove(smallest);
                m_LookUps.Remove(node);
                
                //计算新的Key值并且大于原来key值，这是负优化。下一轮再处理
                var newKey = CalculateKey(node);
                if (smallest.Item1 < newKey)
                {
                    m_OpenSet.Add((newKey, node));
                    m_LookUps[node] = newKey;
                }
                //实际成本>估计值，说明找到了一条更优的
                else if (node.G > node.RHS)
                {
                    node.G = node.RHS;
                    //通知他的上游，更新实际成本
                    foreach (var s in Predecessors(node))
                    {
                        if (s != m_GoalNode)
                        {
                            s.RHS = Mathf.Min(s.RHS, s.Cost(s, node) + node.G);
                        }

                        UpdateVertex(s);
                    }
                    
                }
                //没有找到更优路径
                //该节点不值得遍历
                else
                {
                    var oldG = node.G;
                    node.G = float.MaxValue;
                    
                    foreach (var s in Predecessors(node).Concat(new []{node}))
                    {
                        if (s.RHS.Approx(s.Cost(s,node) + oldG))
                        {
                            if (s != m_GoalNode)
                            {
                                s.RHS = float.MaxValue;
                            }

                            foreach (var sPrime in Successors(s))
                            {
                                s.RHS = Mathf.Min(s.RHS, s.Cost(s, sPrime) + sPrime.G);
                            }
                        }
                        UpdateVertex(s);
                    }
                }
            }

            m_StartNode.G = m_StartNode.RHS;
            Debugger.EO_Log("最短路径已找到，查找" + (k_maxCycles - maxSteps) + "步。");

        }

        //获取节点前驱
        private IEnumerable<Node<T>> Predecessors(Node<T> node)
        {
            return node.Neighbor;
        }
        //获取节点后继
        private IEnumerable<Node<T>> Successors(Node<T> node)
        {
            return node.Neighbor;
        }


        private void Reset()
        {
            m_OpenSet.Clear();
            m_LookUps.Clear();
            km = 0;
            
            foreach (var node in m_AllNodes)
            {
                node.G = float.MaxValue;
                node.RHS = float.MaxValue;
            }
            
            m_GoalNode.RHS = 0;
        }
    }
}